//
// Created by bit on 2022/8/9.
//

#ifndef SITLGUIDANCE_BLOSSIM_H
#define SITLGUIDANCE_BLOSSIM_H

#pragma once

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <iostream>
#include <cmath>
#include "mavlink/v2.0/standard/mavlink.h"
#include "zmq_client.h"
#include "get_time.h"
#include "px4_interface.h"
#include "geo/geo.h"

struct LineOfSight {
    uint32_t time_stamp_ms;
    float vertical;
    float horizontal;
};

/**
 * 在没有视觉信息输入的情况下，根据目标位置和飞机位置模拟出视线角。
 */
void BLOS_calc(const mavlink_local_position_ned_t &local_pos, const mavlink_attitude_quaternion_t &att_q,
               float N_target, float E_target, float D_target, float &vangle, float &hangle);

void LOS_calc(const mavlink_local_position_ned_t &vehicle, float N_target, float E_target, float D_target,
              float &vangle,
              float &hangle);

void pos_to_BLOS(const UavStatus &px4_info,
                 const TargetInfo &target_pos,
                 LineOfSight &blos);

void BLOS2LOS(const mavlink_attitude_quaternion_t &att, const LineOfSight &BLOS,
              float &LOS_vertical, float &LOS_horizon);

float countRad(float a);

//class LineOfSightSim {
//public:
//    LineOfSightSim();
//    ~LineOfSightSim();
//
//    // 体视线角更新
//    bool blos_updated(LineOfSight &blos, LineOfSight &blos_rate);
//
//    // 方位角更新
//    bool los_upodated(LineOfSight &los, LineOfSight &los_rate);
//
//private:
//    ZmqClient    target_info_getter;
//    TargetInfo   target_info;
//
//};


#endif //SITLGUIDANCE_BLOSSIM_H
